﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using OpenNI;
using System.Diagnostics;
using OX.Copyable;

namespace BTKinect.DataType
{
    class KinectData
    {
        #region MEMBERS
        //definitions
        private const string tab = " \t ";

        public readonly List<SkeletonJoint> _LeftKnee = new List<SkeletonJoint> { SkeletonJoint.LeftHip, SkeletonJoint.LeftKnee, SkeletonJoint.LeftFoot };
        public readonly List<SkeletonJoint> _LeftElbow = new List<SkeletonJoint> { SkeletonJoint.LeftShoulder, SkeletonJoint.LeftElbow, SkeletonJoint.LeftHand };
        public readonly List<SkeletonJoint> _LeftShoulder = new List<SkeletonJoint> { SkeletonJoint.RightShoulder, SkeletonJoint.LeftShoulder, SkeletonJoint.LeftElbow };
        public readonly List<SkeletonJoint> _RightKnee = new List<SkeletonJoint> { SkeletonJoint.RightHip, SkeletonJoint.RightKnee, SkeletonJoint.RightFoot };
        public readonly List<SkeletonJoint> _RightElbow = new List<SkeletonJoint> { SkeletonJoint.RightShoulder, SkeletonJoint.RightElbow, SkeletonJoint.RightHand };
        public readonly List<SkeletonJoint> _RightShoulder = new List<SkeletonJoint> { SkeletonJoint.LeftShoulder, SkeletonJoint.RightShoulder, SkeletonJoint.RightElbow };

        private List<KinectPoint3D[]> _OptimizedLeftElbow = null;
        private List<KinectPoint3D[]> _OptimizedLeftKnee = null;
        private List<KinectPoint3D[]> _OptimizedRightElbow = null;
        private List<KinectPoint3D[]> _OptimizedRightKnee = null;
        private List<KinectPoint3D[]> _OptimizedRightShoulder = null;
        private List<KinectPoint3D[]> _OptimizedLeftShoulder = null;

        private List<KinectPoint3D[]> _leftElbow = null;
        private List<KinectPoint3D[]> _leftKnee = null;
        private List<KinectPoint3D[]> _rightElbow = null;
        private List<KinectPoint3D[]> _rightKnee = null;
        private List<KinectPoint3D[]> _rightShoulder = null;
        private List<KinectPoint3D[]> _leftShoulder = null;

        private int numberOfOptimisedDataSets = 0;
        private int lengthOptimisedDataset = 0;
        public const int expectedSize = 1800;
        private int _estimatedSize;
        private List<Dictionary<SkeletonJoint, KinectPoint3D>> _data;
        private Dictionary<int, Dictionary<SkeletonJoint, KinectPoint3D>> _dataAndTime;
        private List<int> _time;
        private List<int> _marker;
        private Stopwatch stopwatch;
        private List<SkeletonJoint> _lstOptimisedJoint = null; //keep track of what needs to be optimised
        private List<SkeletonJoint> _lstSelectedJoints = null; //keep track of what needs to be optimised
        private List<string> _lstString = null;
        private bool blMarkerData = false;
        #endregion

        #region CONSTRUCTORS
        public KinectData()
        {
            Set(expectedSize);
        }
        public KinectData(int estimatedSize)
        {
            Set(estimatedSize);
        }
        #endregion

        #region METHODS
        public KinectData Copy()
        {
            KinectData kinectData = new KinectData(this._dataAndTime.Count);
            kinectData._data = (List<Dictionary<SkeletonJoint, KinectPoint3D>>)_data.Copy();
            kinectData._time = (List<int>)_time.Copy();
            kinectData._marker = (List<int>)_marker.Copy();
            kinectData._dataAndTime = (Dictionary<int, Dictionary<SkeletonJoint, KinectPoint3D>>)_dataAndTime.Copy();

            kinectData._OptimizedLeftElbow = (List<KinectPoint3D[]>)_OptimizedLeftElbow.Copy();
            kinectData._OptimizedLeftKnee = (List<KinectPoint3D[]>)_OptimizedLeftKnee.Copy();
            kinectData._OptimizedLeftShoulder = (List<KinectPoint3D[]>)_OptimizedLeftShoulder.Copy();
            kinectData._OptimizedRightElbow = (List<KinectPoint3D[]>)_OptimizedRightElbow.Copy();
            kinectData._OptimizedRightKnee = (List<KinectPoint3D[]>)_OptimizedRightKnee.Copy();
            kinectData._OptimizedRightShoulder = (List<KinectPoint3D[]>)_OptimizedRightShoulder.Copy();

            kinectData.numberOfOptimisedDataSets = numberOfOptimisedDataSets;
            kinectData._lstOptimisedJoint = (List<SkeletonJoint>)_lstOptimisedJoint.Copy();
            kinectData._lstSelectedJoints = (List<SkeletonJoint>)_lstSelectedJoints.Copy();

            kinectData._leftElbow = (List<KinectPoint3D[]>)_leftElbow.Copy();
            kinectData._leftShoulder = (List<KinectPoint3D[]>)_leftShoulder.Copy();
            kinectData._leftKnee = (List<KinectPoint3D[]>)_leftKnee.Copy();
            kinectData._rightElbow = (List<KinectPoint3D[]>)_rightElbow.Copy();
            kinectData._rightShoulder = (List<KinectPoint3D[]>)_rightShoulder.Copy();
            kinectData._rightKnee = (List<KinectPoint3D[]>)_rightKnee.Copy();

            return kinectData;
        }
        public void Set(int estimatedSize)
        {
            _estimatedSize = estimatedSize;
            _lstOptimisedJoint = new List<SkeletonJoint>(6);
            _lstSelectedJoints = new List<SkeletonJoint>(18);
            _lstString = new List<string>(estimatedSize);

            _OptimizedLeftElbow = new List<KinectPoint3D[]>(estimatedSize);
            _OptimizedLeftKnee = new List<KinectPoint3D[]>(estimatedSize);
            _OptimizedRightElbow = new List<KinectPoint3D[]>(estimatedSize);
            _OptimizedRightKnee = new List<KinectPoint3D[]>(estimatedSize);
            _OptimizedRightShoulder = new List<KinectPoint3D[]>(estimatedSize);
            _OptimizedLeftShoulder = new List<KinectPoint3D[]>(estimatedSize);

            _leftElbow = new List<KinectPoint3D[]>(estimatedSize);
            _leftShoulder = new List<KinectPoint3D[]>(estimatedSize);
            _leftKnee = new List<KinectPoint3D[]>(estimatedSize);
            _rightElbow = new List<KinectPoint3D[]>(estimatedSize);
            _rightShoulder = new List<KinectPoint3D[]>(estimatedSize);
            _rightKnee = new List<KinectPoint3D[]>(estimatedSize);

            _data = new List<Dictionary<SkeletonJoint, KinectPoint3D>>(estimatedSize);
            _dataAndTime = new Dictionary<int, Dictionary<SkeletonJoint, KinectPoint3D>>(estimatedSize);
            _time = new List<int>(estimatedSize);
            _marker = new List<int>(estimatedSize);
            _estimatedSize = expectedSize;
            stopwatch = null;
            numberOfOptimisedDataSets = 0;
        }
        public void AddDataToKinectData(Dictionary<SkeletonJoint, SkeletonJointPosition> d)
        {
            Dictionary<SkeletonJoint, KinectPoint3D> temp = new Dictionary<SkeletonJoint, KinectPoint3D>(d.Count);

            if (stopwatch == null)
            {
                stopwatch = new Stopwatch();
                stopwatch.Start();
            }
            int time = (int)stopwatch.ElapsedMilliseconds;

            foreach (var item in d)
            {
                temp.Add(item.Key, (KinectPoint3D)item.Value.Position);
            }
            _data.Add(temp);
            _time.Add(time);
            _dataAndTime.Add(time, temp);
        }
        public void AddDataToKinectData(Dictionary<SkeletonJoint, SkeletonJointPosition> d, bool marker)
        {
            Dictionary<SkeletonJoint, KinectPoint3D> temp = new Dictionary<SkeletonJoint, KinectPoint3D>(d.Count);
            if (stopwatch == null)
            {
                stopwatch = new Stopwatch();
                stopwatch.Start();
            }
            int time = (int)stopwatch.ElapsedMilliseconds;

            foreach (var item in d)
            {
                temp.Add(item.Key, (KinectPoint3D)item.Value.Position);
            }
            _data.Add(temp);
            _time.Add(time);
            _dataAndTime.Add(time, temp);

            if (marker)
                _marker.Add(1);
            else
                _marker.Add(0);
        }
        public void AddDataToKinectData(Dictionary<SkeletonJoint, KinectPoint3D> d)
        {
            if (stopwatch == null)
            {
                stopwatch = new Stopwatch();
                stopwatch.Start();
            }
            int time = (int)stopwatch.ElapsedMilliseconds;

            _data.Add(d);
            _time.Add(time);
            _dataAndTime.Add(time, d);
        }
        public void AddDataToKinectData(Dictionary<SkeletonJoint, KinectPoint3D> d, bool marker)
        {
            if (stopwatch == null)
            {
                stopwatch = new Stopwatch();
                stopwatch.Start();
            }
            int time = (int)stopwatch.ElapsedMilliseconds;

            _data.Add(d);
            _time.Add(time);
            _dataAndTime.Add(time, d);

            if (marker)
                _marker.Add(1);
            else
                _marker.Add(0);
        }
        #endregion

        #region ACCESSORS
        public List<Dictionary<SkeletonJoint, KinectPoint3D>> Data
        {
            set { _data = value; }
            get
            {
                if (_data.Count == 0)
                {
                    //extract data only
                    _data = new List<Dictionary<SkeletonJoint, KinectPoint3D>>(_estimatedSize);
                    foreach (KeyValuePair<int, Dictionary<SkeletonJoint, KinectPoint3D>> kvp in _dataAndTime)
                    {
                        _data.Add(kvp.Value);
                    }
                }
                return _data;
            }
        }
        public Dictionary<int, Dictionary<SkeletonJoint, KinectPoint3D>> DataAndTime
        {
            get { return _dataAndTime; }
            set
            {
                if (value.Count > 15)     //the action on _dataAndTime has to an assigment
                    _dataAndTime = value;
                ;
            }
        }
        public List<int> Time
        {
            get
            {
                if (_time.Count == 0)
                {
                    _time = new List<int>(_estimatedSize);
                    foreach (KeyValuePair<int, Dictionary<SkeletonJoint, KinectPoint3D>> kvp in _dataAndTime)
                    {
                        _time.Add(kvp.Key);
                    }
                }
                return _time;
            }
            set { _time = value; }
        }
        public List<int> Marker
        {
            get { return _marker; }
            set { _marker = value; }
        }

        public List<KinectPoint3D[]> OptimisedElbowLeft
        {
            get { return _OptimizedLeftElbow; }
            set
            {
                if (_OptimizedLeftElbow.Count == 0)
                    numberOfOptimisedDataSets++;
                _OptimizedLeftElbow = value;
            }
        }
        public List<KinectPoint3D[]> OptimisedElbowRight
        {
            get { return _OptimizedRightElbow; }
            set
            {
                if (_OptimizedRightElbow.Count == 0)
                    numberOfOptimisedDataSets++;
                _OptimizedRightElbow = value;
            }
        }
        public List<KinectPoint3D[]> OptimisedKneeRight
        {
            get { return _OptimizedRightKnee; }
            set
            {
                if (_OptimizedRightKnee.Count == 0)
                    numberOfOptimisedDataSets++;
                _OptimizedRightKnee = value;
            }
        }
        public List<KinectPoint3D[]> OptimisedKneeLeft
        {
            get { return _OptimizedLeftKnee; }
            set
            {
                if (_OptimizedLeftKnee.Count == 0)
                    numberOfOptimisedDataSets++;
                _OptimizedLeftKnee = value;
            }
        }
        public List<KinectPoint3D[]> OptimisedShoulderLeft
        {
            get { return _OptimizedLeftShoulder; }
            set
            {
                if (_OptimizedLeftShoulder.Count == 0)
                    numberOfOptimisedDataSets++;
                _OptimizedLeftShoulder = value;
            }
        }
        public List<KinectPoint3D[]> OptimisedShoulderRight
        {
            get { return _OptimizedRightShoulder; }
            set
            {
                if (_OptimizedRightShoulder.Count == 0)
                    numberOfOptimisedDataSets++;
                _OptimizedRightShoulder = value;
            }
        }

        public List<KinectPoint3D[]> ElbowLeft
        {
            get { return _leftElbow; }
            set
            {
                _leftElbow = value;
            }
        }
        public List<KinectPoint3D[]> ElbowRight
        {
            get { return _rightElbow; }
            set
            {
                _rightElbow = value;
            }
        }
        public List<KinectPoint3D[]> KneeRight
        {
            get { return _rightKnee; }
            set
            {
                _rightKnee = value;
            }
        }
        public List<KinectPoint3D[]> KneeLeft
        {
            get { return _leftKnee; }
            set
            {
                _leftKnee = value;
            }
        }
        public List<KinectPoint3D[]> ShoulderLeft
        {
            get { return _leftShoulder; }
            set
            {
                _leftShoulder = value;
            }
        }
        public List<KinectPoint3D[]> ShoulderRight
        {
            get { return _rightShoulder; }
            set
            {
                _rightShoulder = value;
            }
        }

        public List<string> DataInStringForm
        {
            set
            {
                _lstString = value;
            }
            get
            {
                if (_lstString.Count == 0)
                {
                    List<string> lstStringTemp = new List<string>(1);
                    lstStringTemp.Add("  \t\t\t Geen AddDataToKinectData!   \t\t\t");
                    return lstStringTemp;
                }
                else
                    return _lstString;
            }

        }
        public List<SkeletonJoint> SkeletonJointsToBeOptimised
        {
            set { _lstOptimisedJoint = value; }
            get { return _lstOptimisedJoint; }
        }
        public List<SkeletonJoint> SkeletonJointsSelected
        {
            set { _lstSelectedJoints = value; }
            get { return _lstSelectedJoints; }
        }

        public bool DataPresent
        {
            get { return (MaxLengthOptimisedDataset != 0) || StringDataPresent || _dataAndTime.Count != 0; }
        }
        public int NumberOfOptimisedDataSets
        {
            get { return _lstOptimisedJoint.Count; }
        }
        public int MaxLengthOptimisedDataset
        {
            get
            {
                int[] d = new int[6];
                d[0] = _OptimizedLeftElbow.Count;
                d[1] = _OptimizedLeftKnee.Count;
                d[2] = _OptimizedRightElbow.Count;
                d[3] = _OptimizedRightKnee.Count;
                d[4] = _OptimizedRightShoulder.Count();
                d[5] = _OptimizedLeftShoulder.Count();
                lengthOptimisedDataset = d.Max();
                return lengthOptimisedDataset;
            }
        }
        public bool WitMarkerData
        {
            get { return (_marker.Count != 0); }
            set { blMarkerData = value; }
        }
        public bool StringDataPresent
        {
            //if more then 0.5 second of data is present return true
            get { return (_lstString.Count > 15); }
        }
        public bool OptimisedDataPresent
        {
            get
            {
                return (MaxLengthOptimisedDataset != 0);
            }
        }
        public bool AllJointsToBeOptimised
        {
            set
            {
                if (value)
                {
                    _lstOptimisedJoint.Clear();
                    _lstOptimisedJoint.Add(SkeletonJoint.LeftShoulder);
                    _lstOptimisedJoint.Add(SkeletonJoint.LeftElbow);
                    _lstOptimisedJoint.Add(SkeletonJoint.LeftKnee);
                    _lstOptimisedJoint.Add(SkeletonJoint.RightShoulder);
                    _lstOptimisedJoint.Add(SkeletonJoint.RightElbow);
                    _lstOptimisedJoint.Add(SkeletonJoint.RightKnee);
                }
            }
            get
            {
                return (_lstOptimisedJoint.Count == 6);
            }
        }

        public List<SkeletonJoint> getJointsLeftKnee
        {
            get { return _LeftKnee; }
        }
        public List<SkeletonJoint> getJointsLeftShoulder
        {
            get { return _LeftShoulder; }
        }
        public List<SkeletonJoint> getJointsLeftElbow
        {
            get { return _LeftElbow; }
        }
        public List<SkeletonJoint> getJointsRightKnee
        {
            get { return _RightKnee; }
        }
        public List<SkeletonJoint> getJointsRightShoulder
        {
            get { return _RightShoulder; }
        }
        public List<SkeletonJoint> getJointsRightElbow
        {
            get { return _RightElbow; }
        }

        /// <summary>
        /// get other two joints, to, for example calculate the angle
        /// </summary>
        /// <param name="sk"></param>
        /// <returns></returns>
        public List<SkeletonJoint> GetJoints(SkeletonJoint sk)
        {
            switch (sk)
            {
                case SkeletonJoint.LeftElbow:
                    return getJointsLeftElbow;
                case SkeletonJoint.LeftKnee:
                    return getJointsLeftKnee;
                case SkeletonJoint.LeftShoulder:
                    return getJointsLeftShoulder;
                case SkeletonJoint.RightElbow:
                    return getJointsRightElbow;
                case SkeletonJoint.RightKnee:
                    return getJointsRightKnee;
                case SkeletonJoint.RightShoulder:
                    return getJointsRightShoulder;
            }
            return null;
        }
        public string getJointsInStringForm(SkeletonJoint sk)
        {
            StringBuilder strb = new StringBuilder();
            switch (sk)
            {
                case SkeletonJoint.LeftElbow:
                    for (int i = 0; i < 3; i++)
                    {
                        strb.Append(getJointsLeftElbow[i].ToString() + tab);
                    }
                    break;
                case SkeletonJoint.LeftKnee:
                    for (int i = 0; i < 3; i++)
                    {
                        strb.Append(getJointsLeftKnee[i].ToString() + tab);
                    }
                    break;
                case SkeletonJoint.LeftShoulder:
                    for (int i = 0; i < 3; i++)
                    {
                        strb.Append(getJointsLeftShoulder[i].ToString() + tab);
                    }
                    break;
                case SkeletonJoint.RightElbow:
                    for (int i = 0; i < 3; i++)
                    {
                        strb.Append(getJointsRightElbow[i].ToString() + tab);
                    }
                    break;
                case SkeletonJoint.RightKnee:
                    for (int i = 0; i < 3; i++)
                    {
                        strb.Append(getJointsRightKnee[i].ToString() + tab);
                    }
                    break;
                case SkeletonJoint.RightShoulder:
                    for (int i = 0; i < 3; i++)
                    {
                        strb.Append(getJointsRightShoulder[i].ToString() + tab);
                    }
                    break;
                default:
                    break;
            }
            return strb.ToString();
        }
        public string ToString(SkeletonJoint sk, int index)
        {
            StringBuilder strb = new StringBuilder();
            var v = new List<KinectPoint3D[]>();
            switch (sk)
            {
                case SkeletonJoint.LeftElbow:
                    v = _leftElbow;
                    break;
                case SkeletonJoint.LeftKnee:
                    v = _leftKnee;
                    break;
                case SkeletonJoint.LeftShoulder:
                    v = _leftShoulder;
                    break;
                case SkeletonJoint.RightElbow:
                    v = _rightElbow;
                    break;
                case SkeletonJoint.RightKnee:
                    v = _rightKnee;
                    break;
                case SkeletonJoint.RightShoulder:
                    v = _rightShoulder;
                    break;
                default:
                    break;
            }
            for (int i = 0; i < 3; i++)
            {
                strb.Append(v[index][i].X.ToString() + tab);
                strb.Append(v[index][i].Y.ToString() + tab);
                strb.Append(v[index][i].Z.ToString() + tab);
            }
            return strb.ToString();
        }
        #endregion
    }
}
